#include <conio.h>
#include <iostream>
#include <vector>


#include "Eigen/Sparse"
//#include "Eigen/Eigen"

//#include "point_cloud.h"
//#include "Laplacians.h"
#include "heat_functions.h"

using namespace Eigen;

void save_matrix(string _filename, MatrixXd _matrix)
{
	std::ofstream file(_filename);
	if (file.is_open())
	{
		file << _matrix << endl;		
	}
	file.close();
}


//===================== Main function =======================================================
void main(int argc, char* argv[])
{
	//string name = "halfsphere";
	string name = "bunny_nf5k";

	PCloud pc;
	vector<vector<int>> vector_neighbor_id;
	vector<vector<dPoint>> vector_neighbor_d;
	
	string filename = name + ".m";
	pc.read_from_file(filename);  
	//pc.view_points();
	int np = pc.get_cloud_size();
	double avgs = pc.average_size(10);
	double h=avgs*2;

	std::vector<unsigned int> I, J;
	std::vector<double> S, B;

	I.clear();
	J.clear();
	S.clear();
	B.clear();

	cal_sym_pcb_qb(pc, I, J, S, B, vector_neighbor_id, vector_neighbor_d);

	filename = name + "_Q.txt";
	//    save_IJS("pcdlaplace_vd_matrix.txt", I, J, S) ? printf("IJSB true\n") : printf("IJSB false\n");
	printf("saving q matrix to: %s\n\n", filename);
	save_IJS(filename.c_str(), I, J, S) ? printf("save sym IJS true\n") : printf("save sym IJS false\n");	////sparse matrix S with row I, column J 
	
	filename = name + "_B.txt";	
	//    writeDoubleArray(B, "pcdlaplace_vd_mass.txt");
	printf("saving b matrix to: %s\n\n", filename);
	writeDoubleArray(B, filename.c_str());		//// diagonal matrix B. LBO matrix Lp = B^-1*Q. Matrix B is inverible diagonal and Q is summetric => Lp is symmetrizable


	//recreate B and Q
	MatrixXd matrixB = MatrixXd::Zero(np, np);
	for (int i=0; i< B.size(); i++)
	{
		matrixB(i,i) = B[i];
	}
	
	MatrixXd matrixQ = MatrixXd::Zero(np, np);
	for (int i=0; i< S.size(); i++)
	{
		matrixQ(I[i]-1, J[i]-1) = S[i];
	}

	MatrixXd Ut = step1_heat_flow(0.5, 0.125, 2040, matrixB, matrixQ);	////bunny_nf5k
	//Eigen::MatrixXd Ut = step1_heat_flow(1.0, 0.125, 32, matrixB, matrixQ);	////halfsphere
	//Eigen::MatrixXd Ut = step1_heat_flow(1.0, 0.125, 1834, matrixB, matrixQ);	////lion
	filename = name + "_heat.txt";
	save_matrix(filename, Ut);


	filename = name + "_heat_mapping.m";
	vector_mapping(pc, Ut, filename);
	

	vector<MatrixXd> grad_X_div = step2_evaluate_gradient(pc, Ut, vector_neighbor_id, vector_neighbor_d, h);
	MatrixXd grad = grad_X_div[0];
	MatrixXd X = grad_X_div[1];
	MatrixXd div = grad_X_div[2];

	filename = name + "_grad.txt";
	save_matrix(filename, grad);

	filename = name + "_X.txt";
	save_matrix(filename, X);

	filename = name + "_div.txt";
	save_matrix(filename, div);

	//MatrixXd dist = step3_Poison_equation(LBO, div);
	MatrixXd dist = step3_Poison_equation(matrixB, matrixQ, div);	

	double mindist = dist(1, 0);
	for(int i=0; i<dist.rows();i++)
	{
		if (dist(i,0)< mindist)
		{
			mindist = dist(i,0);
		}
	}
	
	if (mindist<0)
	{
		for(int i=0; i<dist.rows();i++)
		{
			dist(i,0) = dist(i,0) - mindist;
		}
	}

	filename = name + "_dist.txt";
	save_matrix(filename, dist);


	filename = name + "_dist_mapping.m";
	vector_mapping(pc, dist, filename);


	//std::cout << "Press any key to exit" << std::endl;
	//_getch();
}